#include <leaf/searchline.h>
#include <avr/pgmspace.h>

typedef int8_t offset_t;
typedef uint8_t sensor_t;
#define SENSOR_EXCEPTION  (offset_t)-128
/* R- Stand for Right Speed Decrease
 * L- Stand for Left  Speed Decrease
 * Prev Meas Process as previous status
 */
static const sensor_t sens_mask = 0x0F;
const offset_t   offset_map[]  __attribute__ ((progmem)) = {
		SENSOR_EXCEPTION,  //0000
		-3,                //0001  R-
		-1,                //0010  R-
		-2,                //0011  R-
		1,                 //0100  L-
		SENSOR_EXCEPTION,  //0101  R-
		0,                 //0110  Center
		-1,                //0111  R-
		3,                 //1000  L-
		SENSOR_EXCEPTION,  //1001  Prev
		SENSOR_EXCEPTION,  //1010  L-
		SENSOR_EXCEPTION,  //1011  Prev
		2,                 //1100  L-
		SENSOR_EXCEPTION,  //1101  Prev
		1,                 //1110  L-
		SENSOR_EXCEPTION   //1111  Prev
};
/*如果传感器发生变化，请重写传感器状态->偏移量的映射函数*/
static offset_t map_offset( sensor_t sens )
{
	return (offset_t) pgm_read_byte( & offset_map[ sens & sens_mask ] );
}

static int max_speed      = 0;
static int min_speed      = 0;
/* 当前为第k次采样，则offsets三个元素为
 * 0 - k-2 次采样
 * 1 - k-1 次采样
 * 2 - k   次采样
 */
static offset_t offsets[3] = {0, 0, 0};
static offset_t offset = 0;
static int8_t    kp = 10;
static int8_t    kd = 0;
static int adjdiv = 1024;
static void get_sample(uint8_t sens);
static int calc_adjustment(int baseSpeed);
static SpeedPair adjust_speed(const SpeedPair *curSpeed);

/*
 * Search Line Function
 * Parameters: Uint8_t sens -- 4-bits sensor value,
 *             curSpeed   -- current speed(left and right)
 */
SpeedPair searchline(uint8_t sens, const SpeedPair *curSpeed){
	get_sample(sens);
	return adjust_speed(curSpeed);
}

/*+ private methods */
/* 处理采样信息 */
static void get_sample(uint8_t sens){
	offset = map_offset( sens );
	if( offset != SENSOR_EXCEPTION ){
		offsets[0] = offsets[1];
		offsets[1] = offsets[2];
		offsets[2] = offset;
	}
}

/* 用PD算法计算调整量 */
static int calc_adjustment(int baseSpeed){
	int32_t adjustment;
	adjustment  = baseSpeed;
	adjustment *= (
			       kp * offsets[2]
			     + ( (offset == SENSOR_EXCEPTION) ? 0 :
			         (kd * (offsets[2] - offsets[1] * 2 + offsets[0])) )
	);
	adjustment /= adjdiv;
	return adjustment;
}

/*调整速度，如果两次要减速的是相同轮，则累加*/
static SpeedPair adjust_speed(const SpeedPair *curSpeed){
	if(max_speed == 0){
		return (SpeedPair){0, 0};
	}
	SpeedPair calcSpeed = {max_speed, max_speed};
	int lastAdjustment = 0;
	if( ABS( offset ) >= 2 ) //大偏差时才积累之前的调整
		lastAdjustment = curSpeed->right - curSpeed->left;
	int adjustment = calc_adjustment( max_speed );
	if(adjustment < 0)
	{
		calcSpeed.right = max_speed + adjustment
			+ ((lastAdjustment < 0) ? lastAdjustment : 0);
		if(calcSpeed.right < min_speed)
			calcSpeed.right = min_speed;  // set floor for speed
		calcSpeed.left   = max_speed;
	}
	else if(adjustment > 0)
	{
		calcSpeed.left = max_speed - adjustment
			- ((lastAdjustment > 0) ? lastAdjustment : 0);
		if(calcSpeed.left < min_speed)
			calcSpeed.left = min_speed;  // set floor for speed
		calcSpeed.right  = max_speed;
	}
	return calcSpeed;
}
/*- private methods */

/*+Accessor Routines*/
void searchline_reset(void){
	offsets[0] = 0;
	offsets[1] = 0;
	offsets[2] = 0;
}
void searchline_set_adjdiv(int div){
	adjdiv = div;
}
int searchline_get_adjdiv(void){
	return adjdiv;
}
void searchline_set_kp(int8_t k){
	kp = k;
}
int8_t searchline_get_kp(void){
	return kp;
}
void searchline_set_kd(int8_t k){
	kd = k;
}
int8_t searchline_get_kd(void){
	return kd;
}

static Bool adjmin = True;
void searchline_set_adj_min( Bool v )
{
	adjmin = v;
}

void searchilne_set_max(int max){
	max_speed = max;
	if( adjmin )
		searchline_set_min( - max );
}
int searchline_get_max(void){
	return max_speed;
}

void searchline_set_min(int val){
	min_speed = val;
}

int searchline_get_min(void){
	return min_speed;
}
/*- Accessor Routines*/


